#ifndef VECTOR3_
#define VECTOR3_

#include <math.h>

namespace Scythe
{
typedef float Real;
//defs
#ifndef PI
#define PI	(3.141592654f)
#endif

#ifndef TWOPI
#define TWOPI (6.283185307f)
#endif

// degrees to radians multiplier
// eg radians = degrees * DEG2RAD;
#ifndef DEG2RAD
#define DEG2RAD (0.017453292f)
#endif

// degrees = radians * RAD2DEG;
#ifndef RAD2DEG
#define RAD2DEG	(57.29577951f)
#endif

class Matrix;

//classes
class Quat
{
public:
	float x,y,z,w;

};

class Vector3
{
public:
	Vector3(){}

	Vector3(float a, float b, float c) {
		x = a;
		y = b;
		z = c;
	}

	const Vector3& operator=(const Vector3& v) {
		x = v.x;
		y = v.y;
		z = v.z;
		return *this;
	}

	float& operator[](int i) { return (&x)[i]; }
	float  operator[](int i) const { return (&x)[i]; }
  
	Vector3 operator -() const { return Vector3(-x, -y, -z); }

	Vector3 operator +(const Vector3 & v) const {
		return Vector3(x + v.x, y + v.y, z + v.z);
	}
 
	Vector3 operator -(const Vector3 & v) const {
		return Vector3(x - v.x, y - v.y, z - v.z);
	}

	Vector3 operator *(float f) const {
		return Vector3(x * f, y * f, z * f);
	}

	Vector3& operator +=(const Vector3& v) {
		x += v.x;
		y += v.y;
		z += v.z;
		return *this;
	}

	Vector3& operator -=(const Vector3& v) {
		x -= v.x;
		y -= v.y;
		z -= v.z;
		return *this;
	}

	float getLength() const {
		return sqrt(x * x + y * y + z * z);
	}


	float x, y, z;
};
class Matrix
{
public:
	Matrix()
	{
		m[0][0] = 1.0f;
		m[0][1] = 0.0f;
		m[0][2] = 0.0f;

		m[1][0] = 0.0f;
		m[1][1] = 1.0f;
		m[1][2] = 0.0f;

		m[2][0] = 0.0f;
		m[2][1] = 0.0f;
		m[2][2] = 1.0f;
	}

	Matrix operator* (const Matrix& rkMatrix) const
    {
		//borrowed from Ogre :)
        Matrix kProd;
        for (int iRow = 0; iRow < 3; iRow++)
        {
            for (int iCol = 0; iCol < 3; iCol++)
            {
                kProd.m[iRow][iCol] =
                    m[iRow][0]*rkMatrix.m[0][iCol] +
                    m[iRow][1]*rkMatrix.m[1][iCol] +
                    m[iRow][2]*rkMatrix.m[2][iCol];
            }
        }
        return kProd;
    }
    Matrix operator- (const Matrix& rkMatrix) const
    {
	    Matrix kDiff;
	    for (size_t iRow = 0; iRow < 3; iRow++)
	    {
		    for (size_t iCol = 0; iCol < 3; iCol++)
		    {
			    kDiff.m[iRow][iCol] = m[iRow][iCol] -
				    rkMatrix.m[iRow][iCol];
		    }
	    }
	    return kDiff;
    }

	Vector3 operator* (const Vector3& rkPoint) const
    {
        Vector3 kProd;
        for (size_t iRow = 0; iRow < 3; iRow++)
        {
            kProd[iRow] =
                m[iRow][0]*rkPoint[0] +
                m[iRow][1]*rkPoint[1] +
                m[iRow][2]*rkPoint[2];
        }
        return kProd;
    }

	void fromQuat(const Quat q)
    {
        float fCos = cos(q.w);
        float fSin = sin(q.w);
        float fOneMinusCos = 1.0-fCos;
        float fX2 = q.x*q.x;
        float fY2 = q.y*q.y;
        float fZ2 = q.z*q.z;
        float fXYM = q.x*q.y*fOneMinusCos;
        float fXZM = q.x*q.z*fOneMinusCos;
        float fYZM = q.y*q.z*fOneMinusCos;
        float fXSin = q.x*fSin;
        float fYSin = q.y*fSin;
        float fZSin = q.z*fSin;

        m[0][0] = fX2*fOneMinusCos+fCos;
        m[0][1] = fXYM-fZSin;
        m[0][2] = fXZM+fYSin;
        m[1][0] = fXYM+fZSin;
        m[1][1] = fY2*fOneMinusCos+fCos;
        m[1][2] = fYZM-fXSin;
        m[2][0] = fXZM-fYSin;
        m[2][1] = fYZM+fXSin;
        m[2][2] = fZ2*fOneMinusCos+fCos;
    }

	Quat toQuat ()
    {
        // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
        // article "Quaternion Calculus and Fast Animation".
		Quat q;
        float fTrace = m[0][0]+m[1][1]+m[2][2];
        float fRoot;

        if ( fTrace > 0.0 )
        {
            // |w| > 1/2, may as well choose w > 1/2
            fRoot = sqrt(fTrace + 1.0f);  // 2w
            q.w = 0.5f*fRoot;
            fRoot = 0.5f/fRoot;  // 1/(4w)
            q.x = (m[2][1]-m[1][2])*fRoot;
            q.y = (m[0][2]-m[2][0])*fRoot;
            q.z = (m[1][0]-m[0][1])*fRoot;
        }
        else
        {
            // |w| <= 1/2
            static int s_iNext[3] = { 1, 2, 0 };
            int i = 0;
			if ( m[1][1] > m[0][0] )
                i = 1;
			if ( m[2][2] > m[i][i] )
                i = 2;
            int j = s_iNext[i];
            int k = s_iNext[j];

			fRoot = sqrt(m[i][i]-m[j][j]-m[k][k] + 1.0f);
            float* apkQuat[3] = { &q.x, &q.y, &q.z };
            *apkQuat[i] = 0.5f*fRoot;
            fRoot = 0.5f/fRoot;
			q.w = (m[k][j]-m[j][k])*fRoot;
			*apkQuat[j] = (m[j][i]+m[i][j])*fRoot;
			*apkQuat[k] = (m[k][i]+m[i][k])*fRoot;
        }
		return q;
    }

    //-----------------------------------------------------------------------
    Matrix inverse () const
    {
        Matrix kInverse;
        Inverse(kInverse,0.0001f);
        return kInverse;
	}

	Vector3 m[3];
private:
	bool Inverse (Matrix& rkInverse, Real fTolerance) const
    {
        // Invert a 3x3 using cofactors.  This is about 8 times faster than
        // the Numerical Recipes code which uses Gaussian elimination.

        rkInverse.m[0][0] = m[1][1]*m[2][2] -
            m[1][2]*m[2][1];
        rkInverse.m[0][1] = m[0][2]*m[2][1] -
            m[0][1]*m[2][2];
        rkInverse.m[0][2] = m[0][1]*m[1][2] -
            m[0][2]*m[1][1];
        rkInverse.m[1][0] = m[1][2]*m[2][0] -
            m[1][0]*m[2][2];
        rkInverse.m[1][1] = m[0][0]*m[2][2] -
            m[0][2]*m[2][0];
        rkInverse.m[1][2] = m[0][2]*m[1][0] -
            m[0][0]*m[1][2];
        rkInverse.m[2][0] = m[1][0]*m[2][1] -
            m[1][1]*m[2][0];
        rkInverse.m[2][1] = m[0][1]*m[2][0] -
            m[0][0]*m[2][1];
        rkInverse.m[2][2] = m[0][0]*m[1][1] -
            m[0][1]*m[1][0];

        Real fDet =
            m[0][0]*rkInverse.m[0][0] +
            m[0][1]*rkInverse.m[1][0]+
            m[0][2]*rkInverse.m[2][0];

        if ( abs(fDet) <= fTolerance )
            return false;

        Real fInvDet = 1.0/fDet;
        for (size_t iRow = 0; iRow < 3; iRow++)
        {
            for (size_t iCol = 0; iCol < 3; iCol++)
                rkInverse.m[iRow][iCol] *= fInvDet;
        }

        return true;
    }
};

//typedef Vector3 Vector;
//typedef Matrix Matrix;
//some handy conversion functions
typedef float mat4_t[16];

void mat4_Mult(const mat4_t a, const mat4_t b, mat4_t &product);
void nx34ToMat4(mat4_t &dst, const Matrix src, const Vector3 srcpos);
void nx33ToMat4(float* dst, const Matrix src);
void mat4ToNx33(const float* m, Matrix &m33);
void nx33ToMat4nt(float* dst, const Matrix src);	//not transposed
void mat4ToNx33nt(const float* m, Matrix &m33);
void mat4Ident(float* m);

void matNxToVec(const Matrix m, Vector3 &v1, Vector3 &v2, Vector3 &v3);
void nxVecToMat(const Vector3 v1, const Vector3 v2, const Vector3 v3, Matrix &m);
void mat33RotateGlobal(float angle, float x, float y, float z, Matrix &m);
void rotateAxis(Vector3 &v, float x, float y, float z, float angle);
//Novode mat33 version of axis rotate
void mat33Rotate(float angle, float x, float y, float z, Matrix &nm);
float getAngle(const Vector3 axis, const Vector3 v1o, const Vector3 v2o);

void swapXYaxis(float* m);
void swapYZaxis(float* m);
}

#endif